//
// Created by QT on 2023/11/16.
//

#ifndef FOC_MOTOR_H
#define FOC_MOTOR_H


#include "main.h"

#include <stdbool.h>
#include "arm_math.h"

#include "encoder.h"
#include "as5600.h"
#include "as5047p.h"
#include "pid.h"
#include "lowpass_filter.h"



typedef struct phase_current
{
    float32_t ia;
    float32_t ib;
    float32_t ic;
} phase_current_t;

typedef struct alphabeta_current
{
    float32_t ialpha;
    float32_t ibeta;
} alphabeta_current_t;

typedef struct dq_current
{
    float32_t id;
    float32_t iq;
} dq_current_t;

typedef struct dq_voltage
{
    float32_t ud;
    float32_t uq;
} dq_voltage_t;

typedef struct alphabeta_voltage
{
    float32_t ualpha;
    float32_t ubeta;
} alphabeta_voltage_t;

typedef struct sincos_theta
{
    float32_t sinTheta;
    float32_t cosTheta;
} sincos_theta_t;


typedef struct pwm
{
    float32_t period;
    uint32_t Tpwm;
    float32_t ta;
    float32_t tb;
    float32_t tc;

    uint32_t ccra;
    uint32_t ccrb;
    uint32_t ccrc;
} pwm_t;


typedef enum
{
    MOTOR_STATUS_IDLE,
    MOTOR_STATUS_INIT,
    MOTOR_STATUS_START,
    MOTOR_STATUS_RUN,
    MOTOR_STATUS_STOP,
    MOTOR_STATUS_BRAKE,
    MOTOR_STATUS_WAIT,
    MOTOR_STATUS_FAULT,
} motor_status_t;


typedef struct motor
{
    phase_current_t phaseCurrent;
    alphabeta_current_t alphabetaCurrent;
    dq_current_t dqCurrent;
    dq_voltage_t dqVoltage;
    alphabeta_voltage_t alphabetaVoltage;
    sincos_theta_t sincosTheta;


	low_pass_filter_t idLpFilter;
	low_pass_filter_t iqLpFilter;
    pid_ctrl_t dCurrentPid;
    pid_ctrl_t qCurrentPid;
    pid_ctrl_t speedPid;
    pid_ctrl_t positionPid;

    float32_t udc;
    pwm_t pwmParam;

//    as5600_t* encoder;
    encoder_t* encoder;
    //as5047p_t* encoder;

    bool getAngleFlag;
    int fullRotate;
    float32_t theta;
    float32_t angleRad;
    float32_t angleRadOffset;
    float32_t angle, lastAngle;
    uint32_t tick, lastTick;
	low_pass_filter_t speedLdFilter;

    float32_t allElecZeroMapToMechZeroAngle[7];


    /* motor param */
    int polePairNum;
    float32_t Rs, Ld;

    int id;
    motor_status_t status;

} motor_t;


typedef int (*encoder_get_angle_callback_t)(as5047p_t* encoder, float* angle, float* rad);


int motor_create(motor_t* motor);
void motor_set_status(motor_t* motor, motor_status_t status);

void motor_enable(motor_t* motor);
void motor_disable(motor_t* motor);

void motor_startup(motor_t* motor);

bool motor_has_encoder(motor_t* motor);
bool motor_link_encoder(motor_t *motor, encoder_t *encoder);

float32_t motor_get_speed_rpm(motor_t* motor);
float32_t motor_get_angle_rad(motor_t* motor);


void motor_svpwm(motor_t* motor);
void motor_foc_algorithm_step(motor_t* motor);

void motor_set_pwm_duty(motor_t* motor);

float motor_get_elec_angle(motor_t* motor);
void motor_align_encoder(motor_t* motor);
void motor_calib_encoder(motor_t* motor, float32_t* elecZeroAngleRad, int polePair);


void motor_open_loop_test(motor_t* motor, float32_t uq);
void motor_current_closed_loop(motor_t *motor, float32_t id, float32_t iq);

void motor_set_pid_idkp(motor_t* motor, float32_t kp);
void motor_set_pid_idki(motor_t* motor, float32_t ki);


#endif //FOC_MOTOR_H
